/**
 * 2024/03/15
 * ssyyw
 * mavproxy_ros1的飞行控制节点，根据当前飞行计划或指令，输出飞行控制命令到mavros
 *
 */

#include <string>
#include <thread>
#include <atomic>
#include <chrono>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include "mavproxy_ros1/FlightCmd.h"
#include "drone_defs.h"

using namespace std;

const int DEFAULT_QUEUE_SIZE = 20;
const float ENABLE_MESSAGE_DURATION = 5.0;
const float COMMON_ROS_RATE = 20.0;
const float PI = 3.14;

mavros_msgs::State drone_current_state;
// 无人机位置信息
geometry_msgs::PoseStamped drone_current_pose;
// 无人机速度信息
geometry_msgs::TwistStamped drone_current_velocity;
// 无人机当前执行指令
mavproxy_ros1::FlightCmd drone_last_cmd;
// 无人机当前获得指令
mavproxy_ros1::FlightCmd drone_current_cmd;

// 无人机目标位置信息
geometry_msgs::PoseStamped drone_target_pose;
// 无人机速度信息
geometry_msgs::TwistStamped drone_target_velocity;
// 无人机期望姿态信息
geometry_msgs::PoseStamped drone_target_attitude;

// 当前执行的操作索引
int current_task_index;

// ------------------------------------------------------------------------------------------------
void StateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    drone_current_state = *msg;
}

void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    drone_current_pose = *msg;
}

void VelocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    drone_current_velocity = *msg;
}

void CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg)
{
    drone_current_cmd = *msg;
}

ros::Subscriber drone_state_sub;
ros::Subscriber drone_current_pos_sub;
ros::Subscriber drone_current_velocity_sub;
ros::Subscriber drone_current_gpspos_sub;
ros::Subscriber drone_flight_cmd_sub;

ros::Publisher drone_target_pos_pub;
ros::Publisher drone_target_gpos_pub;
ros::Publisher drone_target_velocity_pub;
ros::Publisher drone_target_accel_pub;
ros::Publisher drone_target_mode_pub;
ros::Publisher drone_target_attitude_pub;

ros::ServiceClient drone_arming_client;
ros::ServiceClient drone_mode_client;
ros::ServiceClient drone_takeoff_cmd_client;
ros::ServiceClient drone_land_cmd_client;

// ------------------------------------------------------------------------------------------------
/**
 * @brief 切换无人机模式
 */
int SwitcheModeToTarget(const string &mode_name)
{
    mavros_msgs::SetMode set_mode_message;
    set_mode_message.request.base_mode = 0;
    set_mode_message.request.custom_mode = mode_name;
    if (drone_mode_client.call(set_mode_message) && set_mode_message.response.mode_sent)
    {
        ROS_INFO("setmode send ok %d value:", set_mode_message.response.mode_sent);
        return 0;
    }
    else
    {
        ROS_ERROR("Failed SetMode");
        return 1;
    }
}

/**
 * @brief 解锁无人机
 */
int ArmDrone()
{
    if (drone_current_state.armed == true)
    {
        ROS_INFO("Drone already armed");
        return 0;
    }

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    if (drone_arming_client.call(arm_cmd))
    {
        ROS_INFO("ARM send ok %d, result is %s", arm_cmd.response.success, arm_cmd.response.result);
        return 0;
    }
    else
    {
        ROS_ERROR("Failed arming or disarming");
        return 1;
    }
}

/**
 * @brief 检查两个位置各轴向上的分量距离
 */
bool CheckPosDistance(const geometry_msgs::PoseStamped &current_pos, const geometry_msgs::PoseStamped &target_pos)
{
    if (abs(current_pos.pose.position.x - target_pos.pose.position.x) > 0.2)
    {
        return false;
    }
    if (abs(current_pos.pose.position.x - target_pos.pose.position.x) > 0.2)
    {
        return false;
    }
    if (abs(current_pos.pose.position.x - target_pos.pose.position.x) > 0.2)
    {
        return false;
    }
    return true;
}

/**
 * @brief 发送起飞请求
 * @param 起飞高度
 */
int SendTakeoffMessage()
{
    ROS_INFO("Send takeoff message");

    // 自动起飞模式无法起飞
    // mavros_msgs::CommandTOL takeoff_msg;
    // takeoff_msg.request.altitude = drone_last_cmd.paramdata[0];
    // takeoff_msg.request.latitude = 0;
    // takeoff_msg.request.longitude = 0;
    // takeoff_msg.request.min_pitch = 0;
    // takeoff_msg.request.yaw = 0;
    // if (drone_takeoff_cmd_client.call(takeoff_msg)) {
    //     ROS_INFO("Send takeoff msg ok %d", takeoff_msg.response.success);
    //     return 0;
    // } else {
    //     ROS_ERROR("Failed Takeoff");
    //     return 1;
    // }
    drone_target_pose.pose.position.x = drone_current_pose.pose.position.x;
    drone_target_pose.pose.position.y = drone_current_pose.pose.position.y;
    drone_target_pose.pose.position.z = drone_last_cmd.paramdata[0];

    drone_target_pose.pose.orientation.x = 0;
    drone_target_pose.pose.orientation.y = 0;
    drone_target_pose.pose.orientation.z = 0;
    drone_target_pose.pose.orientation.w = 0;
    return 0;
}

/**
 * @brief 发送降落请求
 * @param
 * 不需要通过TOL类型设置，里面的参数在自动降落模式下基本无意义
 */
int SendLandMessage()
{
    mavros_msgs::CommandTOL land_msg;
    land_msg.request.altitude = drone_current_pose.pose.position.z;
    land_msg.request.latitude = 0;
    land_msg.request.longitude = 0;
    land_msg.request.min_pitch = 0;
    land_msg.request.yaw = 0;
    if (drone_land_cmd_client.call(land_msg))
    {
        ROS_INFO("Send takeoff msg ok %d", land_msg.response.success);
        return 0;
    }
    else
    {
        ROS_ERROR("Failed Takeoff");
        return 1;
    }
}

/**
 * @brief 获取目标姿态位置
*/
int GetTargetPose()
{
    if (drone_last_cmd.paramdata.size() >= 3){
        drone_target_pose.pose.position.x = drone_last_cmd.paramdata[0];
        drone_target_pose.pose.position.y = drone_last_cmd.paramdata[1];
        drone_target_pose.pose.position.z = drone_last_cmd.paramdata[2];
    }

    drone_target_pose.pose.orientation.x = 0;
    drone_target_pose.pose.orientation.y = 0;
    drone_target_pose.pose.orientation.z = 0;
    drone_target_pose.pose.orientation.w = 0;

    return 0;
}

/**
 * @brief 获取目标速度，只控制线速度
*/
int GetTargetVelocity(){
    return 0;
}

/**
 * @brief 获取目标姿态四元数
*/
int GetTargetOrientation(){
    return 0;
}

/**
 * @brief 获取目标姿态
*/
int GetTargetAttitude(){
    return 0;
}

/**
 * @brief 切换Offboard模式下操控位置
 * 官方指导声明不发送位置估计500ms以上，模式切换回上一模式，实际测试不会直接导致模式切换，直接导致的是无人机锁定，再触发安全保护切换至POSITION模式直到落地为止
 */
int SendOffboardPoseMessage()
{
    ros::Rate rate(20.0);

    mavros_msgs::SetMode set_mode_message;
    set_mode_message.request.custom_mode = OFF_BOARD_MODE;

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    while (ros::ok())
    {
        // 请求进入OFFBOARD模式
        if (drone_current_state.mode != OFF_BOARD_MODE && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (drone_mode_client.call(set_mode_message) && set_mode_message.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            // 请求解锁
            if (!drone_current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if (drone_arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        if (ros::Time::now() - last_request > ros::Duration(5.0))
        {
            break;
        }

        drone_target_pos_pub.publish(drone_target_pose);
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

/**
 * @brief 切换Offboard模式下操控速度
 * 官方指导声明不发送位置估计500ms以上，模式切换回上一模式，实际测试不会直接导致模式切换，直接导致的是无人机锁定，再触发安全保护切换至POSITION模式直到落地为止
 */
int SendOffboardVelocityMessage()
{
    return 0;
}

/**
 * 无人机起飞，发送后会自动切换到AUTO.TAKEOFF模式
 */
int LetDroneTakeoff()
{
    ROS_INFO("Enter to takeoff");

    // 不在起飞未成功时设置重试，是因为如果反复向PX4发起飞命令，一旦有一条成功
    // 实际上会导致无人机在起飞成功后，一直不断地从悬停点上升，没找到原因
    ArmDrone();
    SendTakeoffMessage();
    SendOffboardPoseMessage();

    // 将循环发包100次改为起飞后必须硬性等待10s，避免中途打断导致无人机被安全系统锁定
    // sleep(10);

    return 0;
}

/**
 * 无人机降落
 */
int LetDroneLand()
{
    ROS_INFO("Enter to land");

    for (int i = 0; i < GLOBAL_RETRY_TIMES && drone_current_state.mode != LAND_MODE; i++)
    {
        SendLandMessage();
        ros::Duration(1.0).sleep();
    }

    return 0;
}

/**
 * 飞往本地坐标系下的目标地点
 */
int LetDroneFlyToTargetPosition()
{
    GetTargetPose();
    SendOffboardPoseMessage();

    return 0;
}

/**
 * 如果使用setpos来飞行，速度由飞控自己控制
 * 如果此时希望能控制飞行速度，要监控飞行各轴上分量，将分量归一化后乘以目标速度来调整各轴上的期望分量
 */
int LetDroneKeepTargetSpeed(const int &targetLinearSpeed)
{
    geometry_msgs::Twist speed_msg;
    speed_msg.linear.x = targetLinearSpeed;

    drone_target_velocity_pub.publish(speed_msg);
    return 0;
}

/**
 * @brief 让无人机悬停
 * 本来有悬停模式的，但是感觉多此一举，将目标位置设定为原地，效果是一样的。
 */
int LetDroneHover()
{
    drone_target_pose = drone_current_pose;
    return 0;
}

/**
 * 让无人机水平旋转，非水平面上旋转通过俯仰角控制
 */
int LetDroneRotate()
{
    GetTargetVelocity();
    return 0;
}

// 执行指定索引的任务，任务通过planner制定。
int LetDroneWorkFollowPlan(const int &planIndex)
{
    return 0;
}

// 当获得新命令后执行新命令
void HandleCmd()
{
    ROS_INFO("Enter to handle command");

    if (drone_current_cmd.cmdtype == "flight_takeoff")
    {
        LetDroneTakeoff();
        return;
    }
    if (drone_current_cmd.cmdtype == "flight_targetpos")
    {
        LetDroneFlyToTargetPosition();
        return;
    }
    if (drone_current_cmd.cmdtype == "flight_land")
    {
        LetDroneLand();
        return;
    }
}

// 主工作循环
void run()
{
    // 连接无人机
    ros::Rate rate(20.0);
    while (ros::ok() && !drone_current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    while (ros::ok())
    {
        if (drone_last_cmd != drone_current_cmd)
        {
            drone_last_cmd = drone_current_cmd;
            HandleCmd();
        }
        // 处于Offboard模式下时，必须保持发包，因为此时PX4相当于将ROS发过去消息视作了RC，丢失消息相当于丢失RC，
        // 而丢失RC会触发安全保护，直接进入锁定状态且此时无法通过消息唤醒，无人机会进入POSITION模式并逐步下降，直到落地停机。
        if (drone_current_state.mode == OFF_BOARD_MODE)
        {
            drone_target_pos_pub.publish(drone_target_pose);
        }
        else if (drone_current_state.mode != TAKEOFF_MODE && drone_current_state.mode != LAND_MODE)
        {
            // drone_target_pos_pub.publish(drone_current_pose);
            // drone_target_velocity_pub.publish(drone_current_velocity);
        }

        ros::spinOnce();
        rate.sleep();
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "flight_controller");
    ros::NodeHandle nh;

    current_task_index = 0;

    // 订阅MAVROS获得的无人机状态
    drone_state_sub = nh.subscribe<mavros_msgs::State>(MAVROS_INFO_STATE, DEFAULT_QUEUE_SIZE, StateCallback);
    // 订阅无人机当前坐标系下的位置
    drone_current_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVROS_INFO_LOCAL_POSE, DEFAULT_QUEUE_SIZE, PoseCallback);
    // gps 未接入暂时不用
    // drone_current_gpspos_sub = nh.subscribe<mavproxy_ros1::FlightCmd>(MAVROS_INFO_GPS_POS, DEFAULT_QUEUE_SIZE, CmdCallback);
    // 订阅无人机当前的线、角速度
    drone_current_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>(MAVROS_INFO_VELOCITY, DEFAULT_QUEUE_SIZE, VelocityCallback);
    // 订阅无人机基础操作命令，由planner将文本复杂命令拆解下发
    drone_flight_cmd_sub = nh.subscribe<mavproxy_ros1::FlightCmd>(MAVPROXY_DIRECT_CMD, DEFAULT_QUEUE_SIZE, CmdCallback);

    // 发布无人机目标模式，无用
    // drone_target_mode_pub = nh.advertise<mavproxy_ros1::DroneTargetMode>(MAVPROXY_TARGET_MODE, DEFAULT_QUEUE_SIZE);
    // 发布无人机目标位置，指定飞行时使用
    drone_target_pos_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_LOCAL_POSE, DEFAULT_QUEUE_SIZE);
    // gps 未接入暂时不用
    // drone_target_gpos_pub = nh.advertise<geographic_msgs::GeoPoseStamped>(MAVROS_SET_GLOABL_POSE, DEFAULT_QUEUE_SIZE);
    // 发布无人机目标线、角速度，同无人机目标位置主题控制冲突，暂时不用
    drone_target_velocity_pub = nh.advertise<geometry_msgs::Twist>(MAVROS_SET_VELOCITY, DEFAULT_QUEUE_SIZE);
    // 发布无人机目标加速度，非常难用，很多issue和博客指出这个主题基本不可用，至少在PX4上这样
    drone_target_accel_pub = nh.advertise<geometry_msgs::Vector3Stamped>(MAVROS_SET_ACCEL, DEFAULT_QUEUE_SIZE);
    // 发布无人机期望姿态，目前只实现旋转偏航角
    drone_target_attitude_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_ATTITUDE, DEFAULT_QUEUE_SIZE);

    // 无人机加解锁客户端
    drone_arming_client = nh.serviceClient<mavros_msgs::CommandBool>(MAVROS_SRV_ARM);
    // 无人机模式设定客户端
    drone_mode_client = nh.serviceClient<mavros_msgs::SetMode>(MAVROS_SRV_MODE);
    // 无人机起飞服务客户端
    drone_takeoff_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_TAKEOFF);
    // 无人机自动降落模式客户端
    drone_land_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_LAND);

    ROS_INFO("Initializing...");
    ros::Rate rate(20);
    while (ros::ok() && !drone_current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }
    SwitcheModeToTarget(GUIDED_MODE);
    ArmDrone();
    ROS_INFO("Initializd success.");

    run();

    return 0;
}